﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace CPF_experement
{
    static class Travor_A_star
    {
        static Travor_WorldState root;
        public static bool diagonal;
        public static BinaryHeap<Travor_WorldState> openList;
        public static HashSet<Travor_WorldState> closedList;

        static public void init_Travor_A_star(AgentState[] allAgentsState, bool i_diagonal)
        {
            diagonal = i_diagonal;
            closedList = new HashSet<Travor_WorldState>();
            root = new Travor_WorldState(allAgentsState);
            openList = new BinaryHeap<Travor_WorldState>();
            closedList.Add(root);
            openList.Add(root);
        }

        public static bool solve()
        {
            Travor_WorldState temp;
            while (openList.Count > 0)
            {
                temp = openList.Remove();
                if (temp.goalTest())
                {
                    printSolution(temp);
                    return true;
                }
                temp.expandNode();
            }
            return false;
        }
        private static void printSolution(Travor_WorldState end)
        {
            int step = 0;
            Console.WriteLine("solution back to front");
            while (end != null)
            {
                Console.WriteLine("step " + step);
                Console.WriteLine(end.ToString());
                step++;
                end = end.prevStep;
            }
        }
    }
}
